#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/MatrixFunctions>

template <typename T>
using Mat3 = typename Eigen::Matrix<T, 3, 3>;

template <typename T>
Mat3<typename T::Scalar> vectorToSkewMat(const Eigen::MatrixBase<T> &v)
{
    static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 3, "Must have 3x1 matrix");
    Mat3<typename T::Scalar> m;
    m << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0;
    return m;
}

template <typename T>
T pow2(const T &x)
{
    return x * x;
}

template <typename T>
Eigen::Matrix<T, 3, 3> hat(const Eigen::Matrix<T, 3, 1> &vec)
{
    Eigen::Matrix<T, 3, 3> mat;
    T a1 = vec(0);
    T a2 = vec(1);
    T a3 = vec(2);
    mat(0, 0) = T(0);
    mat(0, 1) = -a3;
    mat(0, 2) = a2;
    mat(1, 0) = a3;
    mat(1, 1) = T(0);
    mat(1, 2) = -a1;
    mat(2, 0) = -a2;
    mat(2, 1) = a1;
    mat(2, 2) = T(0);
    return mat;
}

template <typename T>
Eigen::Matrix<T, 3, 3> exp_SO3(const Eigen::Matrix<T, 3, 1> &so3)
{
    assert(so3.size() == 3);
    T theta, A, B;

    theta = sqrt(T(so3.transpose() * so3));
    if (theta < 0.001)
    {
        A = 1.0 - pow(theta, 2) / T(3 * 2);
        B = 1.0 / 2 - pow(theta, 2) / T(4 * 3 * 2);
    }
    else
    {
        A = sin(theta) / theta;
        B = (T(1) - cos(theta)) / pow(theta, 2);
    }

    Eigen::Matrix<T, 3, 3> I = Eigen::Matrix<T, 3, 3>::Identity();
    Eigen::Matrix<T, 3, 3> R = I + A * hat(so3) + B * hat(so3) * hat(so3);

    return R;
}

template <typename T>
Eigen::Matrix<T, 3, 1> log_SO3(const Eigen::Matrix<T, 3, 3> &SO3)
{

    Eigen::Matrix<T, 3, 1> result;
    const T cos_angle = (SO3(0, 0) + SO3(1, 1) + SO3(2, 2) - 1.0) * 0.5;
    result(0, 0) = (SO3(2, 1) - SO3(1, 2)) / 2;
    result(1, 0) = (SO3(0, 2) - SO3(2, 0)) / 2;
    result(2, 0) = (SO3(1, 0) - SO3(0, 1)) / 2;

    T sin_angle_abs = sqrt(result.transpose() * result);

    if (cos_angle > M_SQRT1_2)
    { // [0 - Pi/4[ use asin
        if (sin_angle_abs > 0)
        {
            result *= asin(sin_angle_abs) / sin_angle_abs;
        }
    }
    else if (cos_angle > -M_SQRT1_2)
    { // [Pi/4 - 3Pi/4[ use acos, but antisymmetric part
        const T angle = acos(cos_angle);
        result *= angle / sin_angle_abs;
    }
    else
    { // rest use symmetric part
        // antisymmetric part vanishes, but still large rotation, need information from symmetric part
        const T angle = M_PI - asin(sin_angle_abs);
        const T d0 = SO3(0, 0) - cos_angle,
                d1 = SO3(1, 1) - cos_angle,
                d2 = SO3(2, 2) - cos_angle;
        Eigen::Matrix<T, 3, 1> r2;

        if (d0 * d0 > d1 * d1 && d0 * d0 > d2 * d2)
        { // first is largest, fill with first column
            r2(0, 0) = d0;
            r2(1, 0) = (SO3(1, 0) + SO3(0, 1)) / 2;
            r2(2, 0) = (SO3(0, 2) + SO3(2, 0)) / 2;
        }
        else if (d1 * d1 > d2 * d2)
        { // second is largest, fill with second column
            r2(0, 0) = (SO3(1, 0) + SO3(0, 1)) / 2;
            r2(1, 0) = d1;
            r2(2, 0) = (SO3(2, 1) + SO3(1, 2)) / 2;
        }
        else
        { // third is largest, fill with third column
            r2(0, 0) = (SO3(0, 2) + SO3(2, 0)) / 2;
            r2(1, 0) = (SO3(2, 1) + SO3(1, 2)) / 2;
            r2(2, 0) = d2;
        }
        // flip, if we point in the wrong direction!
        if (r2.transpose() * result < 0)
            r2 *= -1;
        // normalize the vector
        r2.normalize();
        // result = TooN::operator*(angle,r2);
        result = angle * r2;
    }
    return result;
}
